﻿// (c) Copyright Microsoft Corporation.
// This source is subject to the Microsoft Public License (Ms-PL).
// Please see http://go.microsoft.com/fwlink/?LinkID=131993 for details.
// All other rights reserved.

using System;

namespace Coding4Fun.Chair.Robot
{
    public class Jellybean : IDisposable
    {
        readonly RelayController _relayController;
        readonly MotorController _leftMotorController;
        readonly MotorController _rightMotorController;
        
        public const int RaiseFootrestRelayIndex = 0;
        public const int LowerFootrestRelayIndex = 1;

        public Jellybean() : this(5, 10, false) { }

        public Jellybean(int leftSerialPort, int rightSerialPort, bool isMotorEnabled)
        {
            _relayController = new RelayController();
            _leftMotorController = new MotorController(leftSerialPort, IsMotorEnabled);
            _rightMotorController = new MotorController(rightSerialPort, IsMotorEnabled);

            // will turn on motor controllers
            IsMotorEnabled = isMotorEnabled;
        }

        #region public properties
        private bool _isMotorEnabled;
        public bool IsMotorEnabled
        {
            get { return _isMotorEnabled; }
            set
            {
                _isMotorEnabled = value;
                if (_leftMotorController == null || _rightMotorController == null)
                    return;

                PrepSpeedControllers(_leftMotorController);
                PrepSpeedControllers(_rightMotorController);
            }
        }

        private double _maxThrottle = 1;
        public double MaxThrottle
        {
            get { return _maxThrottle; }
            set
            {
                _maxThrottle = Math.Abs(VerifyLegalValues(value));
                AllowedMovementArea = AllowedMovementArea;
            }
        }


        private int _timeToMoveChair = 7500;
        private const int TimeExtraToDeclineChair = 500;
        public int TimeToMoveChair
        {
            get { return _timeToMoveChair; }
            set { _timeToMoveChair = value; }
        }

        public bool IsChairFootrestRaised { get; set; }

        private double _allowedMovementArea;
        private double _negatedAllowedMovementArea = 1;
        public double AllowedMovementArea
        {
            get { return _allowedMovementArea; }
            set
            {
                _allowedMovementArea = Math.Abs(VerifyLegalValues(value));
                if (AllowedMovementArea != 0)
                    _negatedAllowedMovementArea = MaxThrottle / (1 - AllowedMovementArea);
                else
                    _negatedAllowedMovementArea = MaxThrottle;
            }

        }
        #endregion

        #region public methods
        public void ToggleFootrest()
        {
            if (IsChairFootrestRaised)
                DeclineFootrest();
            else
                InclineFootrest();
        }

        public void InclineFootrest()
        {
            if (IsChairFootrestRaised)
                return;

            IsChairFootrestRaised = true;
            if (_relayController.GetTimeDelay(0) > 0)
                return;

            var timeDelay = _relayController.GetTimeDelay(1);

            if (timeDelay < TimeExtraToDeclineChair)
                timeDelay = 0;
            else
                timeDelay -= TimeExtraToDeclineChair;

            _relayController.AlterTimeDelay(1, 0);
            _relayController.Open(0, TimeToMoveChair - timeDelay);
        }

        public void DeclineFootrest()
        {
            if (_relayController.GetTimeDelay(1) > 0)
                return;

            IsChairFootrestRaised = false;
            
            var timeDelay = _relayController.GetTimeDelay(0);
            _relayController.AlterTimeDelay(0, 0);
            
            _relayController.Open(1, TimeToMoveChair - timeDelay + TimeExtraToDeclineChair);
        }


        public Speed CalculateSpeed(double leftSideThrottle, double leftSideVectorMultiplier, double rightSideThrottle, double rightSideVectorMultiplier)
        {
            leftSideThrottle = VerifyLegalValues(leftSideThrottle);
            leftSideVectorMultiplier = VerifyLegalValues(leftSideVectorMultiplier);
            rightSideThrottle = VerifyLegalValues(rightSideThrottle);
            rightSideVectorMultiplier = VerifyLegalValues(rightSideVectorMultiplier);

            leftSideThrottle = AdjustValueForDeadzone(leftSideThrottle);
            leftSideVectorMultiplier = AdjustValueForDeadzone(leftSideVectorMultiplier);
            rightSideThrottle = AdjustValueForDeadzone(rightSideThrottle);
            rightSideVectorMultiplier = AdjustValueForDeadzone(rightSideVectorMultiplier);

            return new Speed()
            {
                // logic flipped to prevent wheel spin for straffing, should be solid since values need to be kitty corner
                LeftMotorFront = VerifyLegalMotorValues(ThrottlesThroughVectorMultiplier(leftSideThrottle, leftSideVectorMultiplier, true)),
                LeftMotorBack = -VerifyLegalMotorValues(ThrottlesThroughVectorMultiplier(leftSideThrottle, leftSideVectorMultiplier, false)),

                RightMotorFront = VerifyLegalMotorValues(ThrottlesThroughVectorMultiplier(rightSideThrottle, -rightSideVectorMultiplier, true)),
                RightMotorBack = -VerifyLegalMotorValues(ThrottlesThroughVectorMultiplier(rightSideThrottle, -rightSideVectorMultiplier, false))
            };
        }

        public void Drive(double leftSideThrottle, double leftSideVectorMultiplier, double rightSideThrottle, double rightSideVectorMultiplier)
        {
            Drive(CalculateSpeed(leftSideThrottle, leftSideVectorMultiplier, rightSideThrottle, rightSideVectorMultiplier));
        }

        public void Drive(Speed speed)
        {
            // speed controller value is between -1000 and 1000
            speed.LeftMotorFront *= MotorController.MaxMotorControllerSpeed;
            speed.LeftMotorBack *= MotorController.MaxMotorControllerSpeed;
            speed.RightMotorFront *= MotorController.MaxMotorControllerSpeed;
            speed.RightMotorBack *= MotorController.MaxMotorControllerSpeed;

            _leftMotorController.SetMotorSpeed((int)speed.LeftMotorFront, -(int)speed.LeftMotorBack);
            _rightMotorController.SetMotorSpeed(-(int)speed.RightMotorBack, (int)speed.RightMotorFront); // due to how the jellybeans are wired
        }
        #endregion

        #region helper functions
        private void PrepSpeedControllers(MotorController controller)
        {
            controller.IsComportEnabled = IsMotorEnabled;

            if (IsMotorEnabled)
                controller.Open();
            else
                controller.Close();
        }

        private double AdjustValueForDeadzone(double value)
        {
            // positive value
            if (value > 0)
            {
                // value under threshold
                if (value < AllowedMovementArea)
                    return 0;

                // re-adjust value back to 0 to 1
                // Example: deadzone of .2
                // that means 1 to .8 is only value, jerky movement
                // readjust so .2 = 0 and use same curve
                // that brings value - allowed deadzone movement
                // so if value = 1, that would be 1, .2 = 0
                // values between .2 and 0 would return 0 due to if statement above
                // so our adjusted range is .2 to 0 but need 1 to 0
                // negating allowed deadzone would be 2 in this case
                // which brings us back to 1 to 0 range
                value = (value - AllowedMovementArea) * _negatedAllowedMovementArea;
            }
            else // negative values
            {
                // value under threshold
                if (value > -AllowedMovementArea)
                    return 0;

                value = (value + AllowedMovementArea) * _negatedAllowedMovementArea;
            }

            return value;
        }

        private double VerifyLegalMotorValues(double value)
        {
            return VerifyLegalValues(value, MaxThrottle);
        }
        
        private static double VerifyLegalValues(double value, double absoluteMaximumSpeed = 1)
        {
            return Math.Min(Math.Max(value, -absoluteMaximumSpeed), absoluteMaximumSpeed);
        }

        private static double ThrottlesThroughVectorMultiplier(double throttle, double vectorMultiplier, bool isFrontMotor)
        {
            return vectorMultiplier + ((isFrontMotor) ? throttle : -throttle);
        }
        #endregion

        protected virtual void Dispose(bool disposing)
        {
            if (_relayController != null)
                _relayController.Dispose();

            if (_leftMotorController != null)
                _leftMotorController.Dispose();

            if (_rightMotorController != null)
                _rightMotorController.Dispose();
        }

        public void Dispose()
        {
            Dispose(true);

            GC.SuppressFinalize(this);
        }
    }
}
